#include "Planner.h"
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/point_generators_2.h>
#include "Kd_tree_d.h"
#include <boost/make_shared.hpp>
#include <CGAL/Boolean_set_operations_2.h>
#include "CollisionDetector.h"
#include "Sampler.h"
#include "Prm.h"
#include "Drrt.h"
#include "CollisionDetector2D.h"
#include "prm_2d.h"
#include "Sampler2D.h"
#include "h_graph.h"
#include <boost/thread.hpp>

//#define USE_DRRT
//#define WITH_TIMER

using namespace std;

Planner::Planner(Scene* scene, int time, bool measure, double alpha, vector<vector<Conf> >* path, double* quality)
	:	m_scene(scene), 
		m_maxSecondsToRun(time), 
		m_isCombinedMeasure(measure), 
		m_clearanceAlpha(alpha), 
		m_outputPath(path),
		m_outputQuality(quality),
    m_stop(false)
{
	/*	this method extracts information regarding the scenario from the 
		gui and initializes the fields 
			m_robot_polygons
			m_obstacles
			m_start_confs, m_target_confs
			m_room
			m_robot_num
	*/
	extract_scenario_from_gui();
}

Planner::~Planner() {}

void Planner::do_work(CollisionDetector* collision, HGraph* hgraph) {
  try {
  Sampler* sampler = new Sampler(m_room, collision);
  
int samples = 700;
#ifdef USE_DRRT
  Sampler2D* robot1Sampler = new Sampler2D(m_room, collision->m_robot1_maze_col);
  Sampler2D* robot2Sampler = new Sampler2D(m_room, collision->m_robot2_maze_col);
  int lastFailedSampels = 0;
  int drrtIters = 64;
  int lastFailedDrrt = 0;
# if WITH_TIMER
  while (true) {
# endif
    Prm2D robot1Prm(*collision->m_robot1_maze_col, *robot1Sampler);
    Prm2D robot2Prm(*collision->m_robot2_maze_col, *robot2Sampler);
    PointPair start(m_start_confs[0], m_start_confs[1]);
    PointPair target(m_target_confs[0], m_target_confs[1]);

    if (m_stop) return;
    cout << "Generating first PRM roadmap with sample size: " << samples << endl;
    boost::thread prm1Thread(&Prm2D::generateRoadmap, &robot1Prm,
                             m_start_confs[0], m_target_confs[0], samples, 12);
    robot2Prm.generateRoadmap(m_start_confs[1], m_target_confs[1], samples, 12);
    prm1Thread.join();
    if (m_stop) return;
    cout << "Initializing DRRT\n";
    Drrt drrt(robot1Prm, robot2Prm, *collision->m_robot2_robot1_col, *collision, *sampler, start, target);
    cout << "Running loop with iters: " << drrtIters << endl;
    if (m_stop) return;
    drrt.runLoop(drrtIters);
    if (m_stop) return;
    cout << "Extracting Path\n";
    vector<PointPair> drrtPath = drrt.retrieve_path();
    if (drrtPath.size() == 0) {
      lastFailedSampels = samples;
      lastFailedDrrt = drrtIters;
      samples *= 2;
      drrtIters *= 2;
#     ifdef WITH_TIMER
      continue;
#     else
      return;
#     endif
    }
    samples -= (samples - lastFailedSampels) / 2;
    drrtIters -= (drrtIters - lastFailedDrrt) / 2;
    vector<Point_d> path;
    for (size_t i=0; i<drrtPath.size(); i++) {
      vector<double> v(4);
      v[0] = CGAL::to_double(drrtPath[i].first[0]);
      v[1] = CGAL::to_double(drrtPath[i].first[1]);
      v[2] = CGAL::to_double(drrtPath[i].second[0]);
      v[3] = CGAL::to_double(drrtPath[i].second[1]);
      path.push_back(Point_d(v.size(), v.begin(), v.end()));
    }

    if (m_stop) return;
    hgraph->addPathList(path);
    if (m_stop) return;
# if WITH_TIMER
  }
# endif

#else
  Prm roadmap = Prm(samples, 12, collision,
      sampler, m_start_confs, m_target_confs);
  roadmap.generate_roadmap();
  // retrieve path from PRM
  vector<vector<Conf> > prmPath = roadmap.retrieve_path();
  m_path = prmPath;
  vector<Point_d> path;
    for (size_t i=0; i<prmPath.size(); i++) {
      vector<double> v(4);
      v[0] = CGAL::to_double(prmPath[i][0][0]);
      v[1] = CGAL::to_double(prmPath[i][0][1]);
      v[2] = CGAL::to_double(prmPath[i][1][0]);
      v[3] = CGAL::to_double(prmPath[i][1][1]);
      path.push_back(Point_d(v.size(), v.begin(), v.end()));
    }
  m_pathQuality = 0;
  for (size_t i=1; i < path.size(); ++i) {
    m_pathQuality += collision->cost_of_path(path[i-1], path[i]);
  }
#endif
  } catch (...) {
    cout << "INTERRUPTED..." << endl;
  }
}

/*	This function is invoked by the GUI and is assumed to update the resulting */
void Planner::run() {
  time_t start = time(NULL);
  CollisionDetector collision(&m_robot_polygons, &m_obstacles, m_isCombinedMeasure, m_clearanceAlpha);
  HGraph hgraph(collision);

# ifdef WITH_TIMER
  boost::thread workThread(&Planner::do_work, this, &collision, &hgraph);
  boost::this_thread::sleep(boost::posix_time::seconds(m_maxSecondsToRun-1));
  m_stop = true;
  workThread.interrupt();
# else
  do_work(&collision, &hgraph);
# endif

#ifdef USE_DRRT
  list<Point_d> hgraphPath = hgraph.findPath();
  vector<vector<Conf> > vcpath;
  for (list<Point_d>::const_iterator iter = hgraphPath.begin(); iter != hgraphPath.end(); ++iter) {
    const Point_d& p = *iter;
    vector<Conf> step;
    step.push_back(Point_2(p[0], p[1]));
    step.push_back(Point_2(p[2], p[3]));
    vcpath.push_back(step);
  }
  m_path = vcpath;
  m_pathQuality = 0;
  vector<Point_d> hGraphVector;
  std::copy(hgraphPath.begin(), hgraphPath.end(), std::back_inserter(hGraphVector));
  for (size_t i=1; i < hGraphVector.size(); ++i) {
    m_pathQuality += collision.cost_of_path(hGraphVector[i-1], hGraphVector[i]);
  }
#endif
  update_results();
  cout << "Done running in " << (time(NULL) - start) << " seconds." << endl;
# ifdef WITH_TIMER
  workThread.join();
# endif
}

void Planner::update_results() {
  cout << "Path " << endl;
  for (int i=0; i<m_path.size(); i++) {
    cout << i << " : " << m_path[i][0] << "," << m_path[i][1] << endl;
  }
  cout << "Quality " << m_pathQuality << endl;

  if (m_outputPath != 0) {
    m_outputPath->clear();
    std::copy(m_path.begin(), m_path.end(), std::back_inserter(*m_outputPath));
  }
  if (m_outputQuality != 0) {
    *m_outputQuality = m_pathQuality;
  }
  
	//	run this method when you finish to produce the path
	//	IMPORTANT: the result should be put in m_path
	transform_path_for_gui();
}
